using System;

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using Microsoft.NETMF.BikeComputer.Application;

using GHIElectronics.NETMF.Hardware;

using System.Runtime.CompilerServices;


namespace Microsoft.NETMF.BikeComputer.Hardware
{
    public interface IInclineSensor
    {
        void ZeroInclinometer();
        double GetIncline();
        double GetTemperature();
    }

    public class FakeInclineSensor: IPausable, IInclineSensor
    {
        static int i = 0;
        public void ZeroInclinometer()
        {
            System.Threading.Thread.Sleep(15000);
        }

        public double GetIncline()
        {
            double[] returnValues = new double[] {12.3,13.2,10.5,9.8,4.2,2.1,1.3,-1.9,-2.7 };
            if (i++ == 8) i = 0;
            return returnValues[i];
        }
        public double GetTemperature()
        {
            double[] returnValues = new double[] {22.3,25.2,18.5,19.8,24.2,22.1,21.3,-1.3,-2.7 };
            return returnValues[i];
        }
        #region IPausable
        [MethodImpl(MethodImplOptions.Synchronized)]
        void IPausable.Pause()
        {
           // dont need to do anything
        }
        [MethodImpl(MethodImplOptions.Synchronized)]
        void IPausable.Resume()
        {
            //dont need to do anything
        }
        #endregion 
    }


    public class InclineSensor : IPausable, IInclineSensor
    {
        # region public Methods

        public InclineSensor(ComputerController controller)
        {
            _controller = controller;

            // set up power GPIO
           // powerInclinometer = new OutputPort(GHIElectronics.NETMF.Hardware.EMX.Pin.IO16, true);  // power of PGPIO not working.  Reads all return garbage.

            //set up SPI port
            SPI.Configuration InclinometerConfig = new SPI.Configuration(
             GHIElectronics.NETMF.Hardware.EMX.Pin.IO11,    //use GPIO pin 11 since it is close to SPI1
             false,                                         //set pin 11 low to access the inclinometer
             1,                                             //setup time required in ms 
             1,                                             //hold chip select low for 0 ms after transfer
             true,                                          //clock line is high when chip not selected/communicating
             true,                                          //data is sampled on the rising edge of clock signal
             800,                                           //clock rate in Hz
             SPI.SPI_module.SPI1);                           //select SPI1 channel for communication

            _spiBus = new SPI(InclinometerConfig);

            // set up interrupt port

            _dataReadyGPIO = new InterruptPort(GHIElectronics.NETMF.Hardware.EMX.Pin.IO7, false, Port.ResistorMode.PullDown, Port.InterruptMode.InterruptEdgeHigh);

            //set up the sample time
            WriteCommandToRegister(c_SMPL_TIME_L, c_Sample_Rate, true);

            // set up Data Ready Interrupt
            WriteCommandToRegister(c_MSC_CTRL_L, c_DataReady_Setup, true);
        }


        #region IPausable   
        [MethodImpl(MethodImplOptions.Synchronized)]
        void IPausable.Pause()
        {
            if (_started)
            {
                PauseSensors();
                _started = false;
            }
        }
        [MethodImpl(MethodImplOptions.Synchronized)]
        void IPausable.Resume()
        {
            if (!_started)
            {
                StartSensors();
                _started = true;
            }
        }
        #endregion 
        
        [MethodImpl(MethodImplOptions.Synchronized)] 
        public void ZeroInclinometer()
        {
            ushort _registerContents;
            byte _byteAverageCount;

            //first get current AverageCount and store so can temporarily replace

            _registerContents = ReadFromRegister(c_AVG_CNT);

            //set the AvereCount for large sample size and get current incline
            //print "Keep bike still and level for a minute" warning when calling this
            WriteCommandToRegister(c_AVG_CNT, c_Set_Ave_Cnt256, true);

            System.Threading.Thread.Sleep(15000);

            //write current incline so it is the new zero level
            WriteCommandToRegister(c_COMMAND_L, c_Set_Zero_Incline, false);

            //reset the previous AverageCount value
            _byteAverageCount = (byte)(_registerContents | c_Ave_Cnt_Mask);
            WriteCommandToRegister(c_COMMAND_L, _byteAverageCount, false);
        }

        [MethodImpl(MethodImplOptions.Synchronized)] 
        public double GetIncline()
        {
            ushort registerContents;
            double incline;


            //Get the current incline
            registerContents = ReadFromRegister(c_INCL_180_OUT);

            WriteCommandToRegister(c_SLP_CNT_L, c_Sleep_Count, true);  //temporary


            if (((int)registerContents & (int)c_SignBitFilter) > 0)
            {
                incline = ~registerContents & c_InclineFilter;
                incline++;
                incline *= -1;
            }
            else
            {
                incline = registerContents & c_InclineFilter;
            }

            incline = incline * .02778;       //converts to incline in %, * .025 is incline in degrees

            Debug.Print("Incline is: " + incline + "%");


            //Read status register 
            registerContents = ReadFromRegister(c_STATUS);


            if (((int)registerContents & (int)c_StatusOK_Mask) < 1)
            {
                Debug.Print("All Systems Operational");
            }
            else
            {
              if (((int)registerContents & (int)c_SelfTest_Mask) > 0)
              {
                  Microsoft.SPOT.Debug.Assert(false);
              }
              if (((int)registerContents & (int)c_SPIComm_Mask) > 0)
              {
                  Microsoft.SPOT.Debug.Assert(false);
              }
              if (((int)registerContents & (int)c_Control_Mask) > 0)
              {
                  Microsoft.SPOT.Debug.Assert(false); ;
              }
              if (((int)registerContents & (int)c_VoltageSupply_Mask) > 0)
              {
                  Microsoft.SPOT.Debug.Assert(false);
              }

            }

            return incline;
        }

        [MethodImpl(MethodImplOptions.Synchronized)] 
        public double GetTemperature()
        {

            double _temperature;
            ushort registerContents;
            registerContents = ReadFromRegister(c_TEMP_OUT);

            registerContents &= c_TempFilter;

            _temperature = (double)registerContents;

            if (_temperature > 1416 || _temperature < 1065)
            {
                throw new Exception("Error reading temperature");
            }
            else
            {
                _temperature = (((1416 - _temperature) * .47) - 40);

                Debug.Print("Temperature is: " + _temperature.ToString() + " C" );

            }
            return _temperature;
        }


        # endregion

        # region private Methods

        private void StartSensors()
        {
            
                //powerInclinometer.Write(true);
                //System.Threading.Thread.Sleep(10); //make sure the sensor is up and stable

                //set up the interrupt handler
                _dataReadyGPIO.OnInterrupt += new NativeEventHandler(drdy_OnInterrupt);

        }

        private void PauseSensors()
        {

            //powerInclinometer.Write(false);

            //set up the interrupt handler
            _dataReadyGPIO.OnInterrupt -= new NativeEventHandler(drdy_OnInterrupt);


        }

        private void WriteCommandToRegister(byte Register, byte Command, bool Verify)
        {
            ushort[] writeBuffer = new ushort[] { 0 };
            ushort[] readBuffer = new ushort[] { 0 };


            writeBuffer[0] = (ushort)(Register << 8 | Command | c_Set_Write_Bit);
            _spiBus.Write(writeBuffer);
            if (Verify)
            {
                ushort data = ReadFromRegister(Register);
                Debug.Print("Register " + Register.ToString() + " Read - " + data.ToString());

                Debug.Assert(data == Command);
            }
        }

        private ushort ReadFromRegister(byte Register)
        {
            ushort[] writeBuffer = new ushort[] { 0 };
            ushort[] readBuffer = new ushort[] { 0 };
            writeBuffer[0] = (ushort)(Register << 8);
            _spiBus.Write(writeBuffer);
            _spiBus.WriteRead(writeBuffer, readBuffer);

            return readBuffer[0];
        }


        [MethodImpl(MethodImplOptions.Synchronized)] 
        private void drdy_OnInterrupt(uint data1, uint data2, DateTime time)
        {
            try
            {
                _dataReadyGPIO.DisableInterrupt();
                double incline = GetIncline();
                _controller.UpdateIncline(incline);
                // set sleep count
                //WriteCommandToRegister(c_SLP_CNT_L, c_Sleep_Count, true);
            }
            finally
            {
                _dataReadyGPIO.EnableInterrupt();
            }
        }



        # endregion

        # region private data

        //private OutputPort powerInclinometer;
        private SPI _spiBus;
        private InterruptPort _dataReadyGPIO;
        private ComputerController _controller;
        private bool _started;

        # endregion    

        #region constants

        // register addresses
        const byte c_AVG_CNT = 0x38;                //AVE_CNT register address
        const byte  c_COMMAND_L = 0x3E;                // COMMAND register Address
        const byte c_MSC_CTRL_L = 0x34;               //  Miscellaneous register
        const byte c_MSC_CNTL_H = 0x35;
        const byte c_SLP_CNT_L = 0x3A;               //Sleep Count Register
        const byte c_INCL_180_OUT = 0x0F;           //read incline data
        const byte c_STATUS = 0x3C;                 //read Status Register
        const byte c_TEMP_OUT = 0x0B;                //read TempOut data
        const byte c_SMPL_TIME_L = 0x36;

        //command values
        const byte c_Set_Ave_Cnt256 = 0X08;          //to set Average Count to 256
        const byte c_Set_Zero_Incline = 0x01;       //sets current incline to zero
        const byte c_DataReady_Setup = 0x07;        // Enables a data REady interrupt to go out through DIO17
        const byte c_Sleep_Count = 0x14;              // sleep for 20 .5 second cycles
        const byte c_Sample_Rate = 0xFF;            // set the sample rate lower than the default

        //masks
        const ushort c_Ave_Cnt_Mask = 0x00FF;           //mask to get current Ave_Cnt value
        const ushort c_InclineFilter = 0x1FFF;          //mask for incline value
        const ushort c_SignBitFilter = 0x2000;          //mask for sign bit
        const ushort c_SelfTest_Mask = 0x0020;          //mask for self test
        const ushort c_SPIComm_Mask = 0x0008;           //mask for SPI
        const ushort c_Control_Mask = 0x0004;           //mask for Control Register
        const ushort c_VoltageSupply_Mask = 0x0003;     //mask for Voltage Supply
        const ushort c_StatusOK_Mask = 0x0020;          //mask for all good status
        const ushort c_TempFilter = 0x0FFF;             //mask for TempOut data

        //Misc
        const ushort c_Set_Write_Bit = 0x8000;           // this is the write bit for write operations

        #endregion


    }
}
